#include "RobotUtils.h"
#include <fstream>
#include <sstream>
#include <boost/lexical_cast.hpp>

using namespace std;

RobotUtils::RobotUtils(void) {
}

RobotUtils::~RobotUtils(void) {
}

void RobotUtils::readCSV(const string &fileName, csvVector &output) {
    ifstream inFile;
    inFile.open(fileName);

    string csvLine;
    // read every line from the stream
    while (getline(inFile, csvLine)) {
        istringstream csvStream(csvLine);
        vector<string> csvRow;
        string csvElement;
        // read every element from the line that is seperated by commas
        // and put it into the vector or strings
        while (getline(csvStream, csvElement, ',')) {
            //	tolgo i doppi apici, se ci sono
            if (csvElement.at(0) == '"')
                csvElement = csvElement.substr(1);
            if (csvElement.at(csvElement.size() - 1) == '"')
                csvElement = csvElement.substr(0, csvElement.size() - 1);

            csvRow.push_back(csvElement);
        }
        output.push_back(csvRow);
    }

    inFile.close();
}

void RobotUtils::writeCSV(csvVector &input, const string &fileName) {
    ofstream outFile;
    outFile.open(fileName);
    string tmp;

    for (csvVector::iterator i = input.begin(); i != input.end(); ++i) {
        for (vector<string>::iterator j = i->begin(); j != i->end(); ++j) {
            //	racchiudo tra doppi apici
            tmp = "\"";
            tmp.append(*j);
            tmp.append("\"");

            //	stampo la virgola se non sono alla fine
            if (j != i->end() - 1)
                outFile << tmp << ",";
            else
                outFile << tmp << "\n";
        }
    }

    outFile.close();
}

void RobotUtils::readInitialPose(ArPose &initialPose) {
    //	leggo le coordinate del sistema robot rispetto al sistema globale
    string initialParamsFileName = "conf/initialParams.csv";
    csvVector csvInput;
    readCSV(initialParamsFileName, csvInput);

    //	tx		->	traslazione in x del sistema robot rispetto al sistema globale
    //	ty		->	traslazione in y del sistema robot rispetto al sistema globale
    //	theta	->	rotazione del sistema robot rispetto al sistema globale
    double tx, ty, theta;
    csvVector::iterator row = csvInput.begin() + 1;
    try {
        tx = boost::lexical_cast<double>(row->at(0));
        ty = boost::lexical_cast<double>(row->at(1));
        theta = boost::lexical_cast<double>(row->at(2));
    } catch (boost::bad_lexical_cast &exc) {
        printf("%s\n", exc.what());
        initialPose.setX(0);
        initialPose.setY(0);
        initialPose.setTh(0);
        return;
    }

    initialPose.setX(tx);
    initialPose.setY(ty);
    initialPose.setTh(theta);
}

/**	Legge da file l'intervallo di correzione della posizione del robot
 *	
 *	@param CORRECTION_INTERVAL il valore letto e' inserito qui
 */
void RobotUtils::readCorrectionInterval(int &CORRECTION_INTERVAL) {
    string initialParamsFileName = "conf/initialParams.csv";
    csvVector csvInput;
    readCSV(initialParamsFileName, csvInput);

    csvVector::iterator row = csvInput.begin() + 1;
    try {
        CORRECTION_INTERVAL = boost::lexical_cast<int>(row->at(3));
    } catch (boost::bad_lexical_cast &exc) {
        printf("%s\n", exc.what());
        CORRECTION_INTERVAL = 1000;
        return;
    }
}

/**	Legge da file l inclinazione della telecamera
 *	
 *	@param theta il valore letto e' inserito qui
 */
void RobotUtils::readTheta(int &theta) {
    string initialParamsFileName = "conf/initialParams.csv";
    csvVector csvInput;
    readCSV(initialParamsFileName, csvInput);

    csvVector::iterator row = csvInput.begin() + 1;
    try {
        theta = boost::lexical_cast<int>(row->at(4));
    } catch (boost::bad_lexical_cast &exc) {
        printf("%s\n", exc.what());
        theta = 0;
        return;
    }
}

/**	Legge da file una sequenza di obiettivi (punti sul piano rispetto al sistema globale)
 *	
 *	@param goals i valori letti sono inseriti qui
 */
void RobotUtils::readGoals(vector<ArPose> &goals) {
    //	leggo gli obiettivi da raggiungere, in coordinate globali
    string goalsFileName = "conf/goals.csv";
    csvVector csvInput;
    readCSV(goalsFileName, csvInput);
    //	variabili temporanee per leggere le coordinate
    double x, y;
    //	scorro i dati, saltando l'intestazione
    for (csvVector::iterator row = csvInput.begin() + 1; row != csvInput.end(); ++row) {
        try {
            x = boost::lexical_cast<double>(row->at(0));
            y = boost::lexical_cast<double>(row->at(1));
        } catch (boost::bad_lexical_cast &exc) {
            printf("%s\n", exc.what());
            return;
        }
        goals.push_back(ArPose(x, y));
    }
}

/**	Legge da file una sequenza di marker (punti sul piano rispetto al sistema globale)
 *	
 *	@param markers i valori letti sono inseriti qui
 */
void RobotUtils::readMarkers(std::vector<MyMarker> &markers) {
    //	leggo i marker, in coordinate globali
    string markersFileName = "conf/markers.csv";
    csvVector csvInput;
    readCSV(markersFileName, csvInput);
    //	variabili temporanee per leggere le coordinate
    int id;
    double x, y, height;
    //	scorro i dati, saltando l'intestazione
    for (csvVector::iterator row = csvInput.begin() + 1; row != csvInput.end(); ++row) {
        try {
            id = boost::lexical_cast<int>(row->at(0));
            x = boost::lexical_cast<double>(row->at(1));
            y = boost::lexical_cast<double>(row->at(2));
            height = boost::lexical_cast<double>(row->at(3));
        } catch (boost::bad_lexical_cast &exc) {
            printf("%s\n", exc.what());
            return;
        }
        markers.push_back(MyMarker(id, x, y, height));
    }
}
